\hypertarget{classWBC__priority}{}\doxysection{W\+B\+C\+\_\+priority Class Reference}
\label{classWBC__priority}\index{WBC\_priority@{WBC\_priority}}


This class implements a whole body controller with priority control.  




{\ttfamily \#include $<$wbc\+\_\+priority.\+h$>$}



Collaboration diagram for W\+B\+C\+\_\+priority\+:\nopagebreak
\begin{figure}[H]
\begin{center}
\leavevmode
\includegraphics[width=170pt]{classWBC__priority__coll__graph}
\end{center}
\end{figure}
\doxysubsection*{Public Member Functions}
\begin{DoxyCompactItemize}
\item 
\mbox{\hyperlink{classWBC__priority_a8c838694a7fa3038bb56923679050bea}{W\+B\+C\+\_\+priority}} (int model\+\_\+nv\+\_\+\+In, int Q\+P\+\_\+nv\+In, int Q\+P\+\_\+nc\+In, double miu\+\_\+\+In, double dt)
\begin{DoxyCompactList}\small\item\em Construct a new wbc priority object. \end{DoxyCompactList}\item 
void \mbox{\hyperlink{classWBC__priority_ad7a8bfff2aa05dcedeb6e3e656d3c6fd}{set\+Qini}} (const Eigen\+::\+Vector\+Xd \&q\+Ini)
\begin{DoxyCompactList}\small\item\em Set the initial generalized position of robot. \end{DoxyCompactList}\item 
void \mbox{\hyperlink{classWBC__priority_adf9a6a3385417332666e2157b6903fe9}{compute\+Tau}} ()
\begin{DoxyCompactList}\small\item\em compute the desired joint torques, basically the Dyn\+W\+BC \end{DoxyCompactList}\item 
void \mbox{\hyperlink{classWBC__priority_a7ed27e44e3d0044570620d3679f26389}{data\+Bus\+Read}} (const \mbox{\hyperlink{structDataBus}{Data\+Bus}} \&robot\+State)
\begin{DoxyCompactList}\small\item\em read data from databus \end{DoxyCompactList}\item 
void \mbox{\hyperlink{classWBC__priority_a1504c45533bea5c9b6a4a87648db0c0c}{data\+Bus\+Write}} (\mbox{\hyperlink{structDataBus}{Data\+Bus}} \&robot\+State)
\begin{DoxyCompactList}\small\item\em write data to databus \end{DoxyCompactList}\item 
void \mbox{\hyperlink{classWBC__priority_a7cb65e689671b04b1b4bca685602abaa}{compute\+Ddq}} (\mbox{\hyperlink{classPin__KinDyn}{Pin\+\_\+\+Kin\+Dyn}} \&kin\+Solver)
\begin{DoxyCompactList}\small\item\em compute the desired dq, basically the Kin\+W\+BC \end{DoxyCompactList}\end{DoxyCompactItemize}
\doxysubsection*{Public Attributes}
\begin{DoxyCompactItemize}
\item 
\mbox{\Hypertarget{classWBC__priority_a63e9d9ee61900d1c7e6c7d0e023953aa}\label{classWBC__priority_a63e9d9ee61900d1c7e6c7d0e023953aa}} 
int \mbox{\hyperlink{classWBC__priority_a63e9d9ee61900d1c7e6c7d0e023953aa}{model\+\_\+nv}}
\begin{DoxyCompactList}\small\item\em size of the system generalized coordinate dq \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a89bb360d72ea2ceb80bcc714427978a1}\label{classWBC__priority_a89bb360d72ea2ceb80bcc714427978a1}} 
Eigen\+::\+Vector3d \mbox{\hyperlink{classWBC__priority_a89bb360d72ea2ceb80bcc714427978a1}{tau\+\_\+upp\+\_\+L}}
\begin{DoxyCompactList}\small\item\em upper bound of ground reaction moment \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_ad031c3a10312ef84c6f587a80cf7ea16}\label{classWBC__priority_ad031c3a10312ef84c6f587a80cf7ea16}} 
Eigen\+::\+Vector3d \mbox{\hyperlink{classWBC__priority_ad031c3a10312ef84c6f587a80cf7ea16}{tau\+\_\+low\+\_\+L}}
\begin{DoxyCompactList}\small\item\em lower bound of ground reaction moment \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a835eb7c5c5a382770ec08294bb3a072b}\label{classWBC__priority_a835eb7c5c5a382770ec08294bb3a072b}} 
double \mbox{\hyperlink{classWBC__priority_a835eb7c5c5a382770ec08294bb3a072b}{f\+\_\+z\+\_\+low}} \{0\}
\begin{DoxyCompactList}\small\item\em lower bound of ground reaction force along z-\/axis \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a43d29119681f29138bf209c027951505}\label{classWBC__priority_a43d29119681f29138bf209c027951505}} 
double \mbox{\hyperlink{classWBC__priority_a43d29119681f29138bf209c027951505}{f\+\_\+z\+\_\+upp}} \{0\}
\begin{DoxyCompactList}\small\item\em upper bound of ground reaction force along z-\/axis \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a3104af953a95b48eec6a05798f8b0110}\label{classWBC__priority_a3104af953a95b48eec6a05798f8b0110}} 
int \mbox{\hyperlink{classWBC__priority_a3104af953a95b48eec6a05798f8b0110}{leg\+State\+Cur}}
\begin{DoxyCompactList}\small\item\em current leg swing or stance state \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a8cf4c0b317a325f1d8c751a8d2fa282c}\label{classWBC__priority_a8cf4c0b317a325f1d8c751a8d2fa282c}} 
double \mbox{\hyperlink{classWBC__priority_a8cf4c0b317a325f1d8c751a8d2fa282c}{miu}} \{0.\+5\}
\begin{DoxyCompactList}\small\item\em friction coefficient of model \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_aa7fefdac3d147714609ce1bd15edf389}\label{classWBC__priority_aa7fefdac3d147714609ce1bd15edf389}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_aa7fefdac3d147714609ce1bd15edf389}{dyn\+\_\+M}}
\begin{DoxyCompactList}\small\item\em dynamic parameters of the dynamic function M$\ast$ddq+\+C$\ast$dq+G=tau \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_ad9d3684eeccb5f31dcbd0068d2d3b130}\label{classWBC__priority_ad9d3684eeccb5f31dcbd0068d2d3b130}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_ad9d3684eeccb5f31dcbd0068d2d3b130}{dyn\+\_\+\+M\+\_\+inv}}
\begin{DoxyCompactList}\small\item\em inverse of dynamic parameter dyn\+\_\+M \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_ac567541e708cee14da44b5e1ff1cbd11}\label{classWBC__priority_ac567541e708cee14da44b5e1ff1cbd11}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_ac567541e708cee14da44b5e1ff1cbd11}{dyn\+\_\+\+Ag}}
\begin{DoxyCompactList}\small\item\em Centroidal Momentum Matrix. \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a19c84ca7e1268b73b4de35426d026d1c}\label{classWBC__priority_a19c84ca7e1268b73b4de35426d026d1c}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_a19c84ca7e1268b73b4de35426d026d1c}{dyn\+\_\+d\+Ag}}
\begin{DoxyCompactList}\small\item\em Centroidal Momentum Matrix time variant. \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a87e42205ecc21da740cd68dd97dfc53b}\label{classWBC__priority_a87e42205ecc21da740cd68dd97dfc53b}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a87e42205ecc21da740cd68dd97dfc53b}{dyn\+\_\+\+Non}}
\begin{DoxyCompactList}\small\item\em dynamic parameters of the dynamic function M$\ast$ddq+\+C$\ast$dq+G=tau, dyn\+\_\+\+Non=dyn\+\_\+\+C$\ast$dq+dyn\+\_\+G \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a03d1a9c1e92ed193b06b4606b3698d71}\label{classWBC__priority_a03d1a9c1e92ed193b06b4606b3698d71}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_a03d1a9c1e92ed193b06b4606b3698d71}{Jc}}
\begin{DoxyCompactList}\small\item\em jacobian matrix of foot contact \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a690988c51a899e4ed196c80326efcba0}\label{classWBC__priority_a690988c51a899e4ed196c80326efcba0}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_a690988c51a899e4ed196c80326efcba0}{d\+Jc}}
\begin{DoxyCompactList}\small\item\em jacobian time variation matrix of foot contact \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a651f35f4a14fb2dfb41356d69dbaa9c5}\label{classWBC__priority_a651f35f4a14fb2dfb41356d69dbaa9c5}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_a651f35f4a14fb2dfb41356d69dbaa9c5}{Jfe}}
\begin{DoxyCompactList}\small\item\em Jfe = \mbox{[}Jfe\+\_\+\+L.\+transpose() Jfe\+\_\+\+R.\+transpose()\mbox{]}.transpose() \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a364b8179d23d33e89296ad61e352a9db}\label{classWBC__priority_a364b8179d23d33e89296ad61e352a9db}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_a364b8179d23d33e89296ad61e352a9db}{Jfe\+\_\+L}}
\begin{DoxyCompactList}\small\item\em jacobian matrix of left foot \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_ac612a1dc22bf476a8e4fdb430e69eac1}\label{classWBC__priority_ac612a1dc22bf476a8e4fdb430e69eac1}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_ac612a1dc22bf476a8e4fdb430e69eac1}{Jfe\+\_\+R}}
\begin{DoxyCompactList}\small\item\em jacobian matrix of right foot \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_ad4278ea3aca9cc98c8c1ff411204e211}\label{classWBC__priority_ad4278ea3aca9cc98c8c1ff411204e211}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_ad4278ea3aca9cc98c8c1ff411204e211}{J\+\_\+hd\+\_\+l}}
\begin{DoxyCompactList}\small\item\em jacobian matrix of left hand \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_ab2950b524448c63af91ce7b713aec947}\label{classWBC__priority_ab2950b524448c63af91ce7b713aec947}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_ab2950b524448c63af91ce7b713aec947}{J\+\_\+hd\+\_\+r}}
\begin{DoxyCompactList}\small\item\em jacobian matrix of right hand \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a4912d86b01e6bb6b48e4f8ac60f24cce}\label{classWBC__priority_a4912d86b01e6bb6b48e4f8ac60f24cce}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_a4912d86b01e6bb6b48e4f8ac60f24cce}{d\+J\+\_\+hd\+\_\+l}}
\begin{DoxyCompactList}\small\item\em jacobian time variation matrix of left hand \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a5c7a80ed75a31dd24a2732852f12e4b1}\label{classWBC__priority_a5c7a80ed75a31dd24a2732852f12e4b1}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_a5c7a80ed75a31dd24a2732852f12e4b1}{d\+J\+\_\+hd\+\_\+r}}
\begin{DoxyCompactList}\small\item\em jacobian time variation matrix of right hand \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_ae7a229982a7799821b343ba032621054}\label{classWBC__priority_ae7a229982a7799821b343ba032621054}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_ae7a229982a7799821b343ba032621054}{Jsw}}
\begin{DoxyCompactList}\small\item\em foot of swing leg Jacobian \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_aad3ca89c91e596b1a89da5cbd65c1991}\label{classWBC__priority_aad3ca89c91e596b1a89da5cbd65c1991}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_aad3ca89c91e596b1a89da5cbd65c1991}{d\+Jsw}}
\begin{DoxyCompactList}\small\item\em foot of swing leg Jacobian time variation \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a4440d37fa7fdd00e3b3a5df797d2271a}\label{classWBC__priority_a4440d37fa7fdd00e3b3a5df797d2271a}} 
Eigen\+::\+Matrix3d \mbox{\hyperlink{classWBC__priority_a4440d37fa7fdd00e3b3a5df797d2271a}{fe\+\_\+rot\+\_\+sw\+\_\+W}}
\begin{DoxyCompactList}\small\item\em current swing foot rotation matrix in world frame \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a605e33bf1886ad1fe93e120a984b5085}\label{classWBC__priority_a605e33bf1886ad1fe93e120a984b5085}} 
Eigen\+::\+Vector3d \mbox{\hyperlink{classWBC__priority_a605e33bf1886ad1fe93e120a984b5085}{fe\+\_\+pos\+\_\+sw\+\_\+W}}
\begin{DoxyCompactList}\small\item\em current swing foot position in world frame \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_ab25e91d2356c16769cc18c9355338587}\label{classWBC__priority_ab25e91d2356c16769cc18c9355338587}} 
Eigen\+::\+Vector3d \mbox{\hyperlink{classWBC__priority_ab25e91d2356c16769cc18c9355338587}{hd\+\_\+l\+\_\+pos\+\_\+cur\+\_\+W}}
\begin{DoxyCompactList}\small\item\em current left hand position in world frame \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_ada612b26865fc9a977e6cecaa813c6a4}\label{classWBC__priority_ada612b26865fc9a977e6cecaa813c6a4}} 
Eigen\+::\+Vector3d \mbox{\hyperlink{classWBC__priority_ada612b26865fc9a977e6cecaa813c6a4}{hd\+\_\+r\+\_\+pos\+\_\+cur\+\_\+W}}
\begin{DoxyCompactList}\small\item\em current right hand position in world frame \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a377b89be889b9a1d221e041ec6621f0a}\label{classWBC__priority_a377b89be889b9a1d221e041ec6621f0a}} 
Eigen\+::\+Matrix3d \mbox{\hyperlink{classWBC__priority_a377b89be889b9a1d221e041ec6621f0a}{hd\+\_\+l\+\_\+rot\+\_\+cur\+\_\+W}}
\begin{DoxyCompactList}\small\item\em current left hand rotation matrix in world frame \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a380db2c47e2752ebebe6aab083051f4f}\label{classWBC__priority_a380db2c47e2752ebebe6aab083051f4f}} 
Eigen\+::\+Matrix3d \mbox{\hyperlink{classWBC__priority_a380db2c47e2752ebebe6aab083051f4f}{hd\+\_\+r\+\_\+rot\+\_\+cur\+\_\+W}}
\begin{DoxyCompactList}\small\item\em current right hand rotation matrix in world frame \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a4f55ea86d606fb254b0b743161ff6fff}\label{classWBC__priority_a4f55ea86d606fb254b0b743161ff6fff}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a4f55ea86d606fb254b0b743161ff6fff}{q}}
\begin{DoxyCompactList}\small\item\em current robot generalized position \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a8b1444c9875e783ffff14fbbc73a3fc0}\label{classWBC__priority_a8b1444c9875e783ffff14fbbc73a3fc0}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a8b1444c9875e783ffff14fbbc73a3fc0}{dq}}
\begin{DoxyCompactList}\small\item\em current robot generalized velcity \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_adacd4ee75f80b37e9fb0779bf9cf98a7}\label{classWBC__priority_adacd4ee75f80b37e9fb0779bf9cf98a7}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_adacd4ee75f80b37e9fb0779bf9cf98a7}{ddq}}
\begin{DoxyCompactList}\small\item\em current robot generalized acceleration \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_af15ff64b154c8ae1d4567cca12ce01db}\label{classWBC__priority_af15ff64b154c8ae1d4567cca12ce01db}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_af15ff64b154c8ae1d4567cca12ce01db}{Fr\+\_\+ff}}
\begin{DoxyCompactList}\small\item\em feedforward ground reaction force from \mbox{\hyperlink{classMPC}{M\+PC}} module, Fr\+\_\+ff = \mbox{[}fe\+\_\+L, fe\+\_\+R\mbox{]}, dim = 12$\ast$1 \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a328bcfe2189e0f4e6242db7fc945ee06}\label{classWBC__priority_a328bcfe2189e0f4e6242db7fc945ee06}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a328bcfe2189e0f4e6242db7fc945ee06}{delta\+\_\+ddq}}
\begin{DoxyCompactList}\small\item\em delta\+\_\+ddq to correction in Dyn\+W\+BC, N\+OT U\+SE \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a29eb5f45b965f751d8c21983313744b3}\label{classWBC__priority_a29eb5f45b965f751d8c21983313744b3}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a29eb5f45b965f751d8c21983313744b3}{delta\+\_\+\+Fr}}
\begin{DoxyCompactList}\small\item\em delta\+\_\+\+Fr to correction in Dyn\+W\+BC, N\+OT U\+SE \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_adebd8b10447a44efc435f34d2b9d5a0d}\label{classWBC__priority_adebd8b10447a44efc435f34d2b9d5a0d}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_adebd8b10447a44efc435f34d2b9d5a0d}{eigen\+\_\+x\+Opt}}
\begin{DoxyCompactList}\small\item\em optimized x = \mbox{[}ddq, fr\mbox{]} in Dyn\+W\+BC \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_ad7c5607f6d6a468b81cf035396ee1885}\label{classWBC__priority_ad7c5607f6d6a468b81cf035396ee1885}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_ad7c5607f6d6a468b81cf035396ee1885}{eigen\+\_\+ddq\+\_\+\+Opt}}
\begin{DoxyCompactList}\small\item\em final ddq after Dyn\+W\+BC \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a0cfc5cc63af24a7e8d04f4ef360ba20e}\label{classWBC__priority_a0cfc5cc63af24a7e8d04f4ef360ba20e}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a0cfc5cc63af24a7e8d04f4ef360ba20e}{eigen\+\_\+fr\+\_\+\+Opt}}
\begin{DoxyCompactList}\small\item\em final reaction force of two legs after Dyn\+W\+BC \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a78ee8266d45373be050708733b7ac928}\label{classWBC__priority_a78ee8266d45373be050708733b7ac928}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a78ee8266d45373be050708733b7ac928}{eigen\+\_\+tau\+\_\+\+Opt}}
\begin{DoxyCompactList}\small\item\em final joint torques after Dyn\+W\+BC \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_af7eb60ca2fb34b7bcd10f488384e7a95}\label{classWBC__priority_af7eb60ca2fb34b7bcd10f488384e7a95}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_af7eb60ca2fb34b7bcd10f488384e7a95}{Q1}}
\begin{DoxyCompactList}\small\item\em weighted matrix of G\+RF and G\+RM in QP \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a24cd63af227e80cfabfa7f9265a8270d}\label{classWBC__priority_a24cd63af227e80cfabfa7f9265a8270d}} 
Eigen\+::\+Matrix\+Xd \mbox{\hyperlink{classWBC__priority_a24cd63af227e80cfabfa7f9265a8270d}{Q2}}
\begin{DoxyCompactList}\small\item\em weithted matrix of ddq in QP \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a071214fcca162d0a68d66b7eda3d5a77}\label{classWBC__priority_a071214fcca162d0a68d66b7eda3d5a77}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a071214fcca162d0a68d66b7eda3d5a77}{delta\+\_\+q\+\_\+final\+\_\+kin}}
\begin{DoxyCompactList}\small\item\em delta\+\_\+q result from Kin\+W\+BC \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a353a58b77c7d3844899a08eccfdc9630}\label{classWBC__priority_a353a58b77c7d3844899a08eccfdc9630}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a353a58b77c7d3844899a08eccfdc9630}{dq\+\_\+final\+\_\+kin}}
\begin{DoxyCompactList}\small\item\em dq result from Kin\+W\+BC \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a914f1b2e9a0c441a6dab2883e5e932a2}\label{classWBC__priority_a914f1b2e9a0c441a6dab2883e5e932a2}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a914f1b2e9a0c441a6dab2883e5e932a2}{ddq\+\_\+final\+\_\+kin}}
\begin{DoxyCompactList}\small\item\em ddq result from Kin\+W\+BC \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a1193aad27a9cafd2b447a91fcb252103}\label{classWBC__priority_a1193aad27a9cafd2b447a91fcb252103}} 
Eigen\+::\+Vector\+Xd \mbox{\hyperlink{classWBC__priority_a1193aad27a9cafd2b447a91fcb252103}{tau\+Joint\+Res}}
\begin{DoxyCompactList}\small\item\em joint torques result from Kin\+W\+BC \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a783f18f6fb7fdde9d0a098adc816bf44}\label{classWBC__priority_a783f18f6fb7fdde9d0a098adc816bf44}} 
Eigen\+::\+Matrix3d \mbox{\hyperlink{classWBC__priority_a783f18f6fb7fdde9d0a098adc816bf44}{fe\+\_\+\+L\+\_\+rot\+\_\+\+L\+\_\+off}}
\begin{DoxyCompactList}\small\item\em left foot-\/end rotation matrix w.\+r.\+t to the body frame in offset posture \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a480a21e54d4f6d9ce948eaf92c7e6ea4}\label{classWBC__priority_a480a21e54d4f6d9ce948eaf92c7e6ea4}} 
Eigen\+::\+Matrix3d \mbox{\hyperlink{classWBC__priority_a480a21e54d4f6d9ce948eaf92c7e6ea4}{fe\+\_\+\+R\+\_\+rot\+\_\+\+L\+\_\+off}}
\begin{DoxyCompactList}\small\item\em right foot-\/end rotation matrix w.\+r.\+t to the body frame in offset posture \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classWBC__priority_a16c781d380edf8222b2e38225a941bf1}\label{classWBC__priority_a16c781d380edf8222b2e38225a941bf1}} 
\mbox{\hyperlink{classPriorityTasks}{Priority\+Tasks}} \mbox{\hyperlink{classWBC__priority_a16c781d380edf8222b2e38225a941bf1}{kin\+\_\+tasks}}
\begin{DoxyCompactList}\small\item\em kinematic tasks with priority for Kin\+W\+BC \end{DoxyCompactList}\end{DoxyCompactItemize}


\doxysubsection{Detailed Description}
This class implements a whole body controller with priority control. 



Definition at line 24 of file wbc\+\_\+priority.\+h.



\doxysubsection{Constructor \& Destructor Documentation}
\mbox{\Hypertarget{classWBC__priority_a8c838694a7fa3038bb56923679050bea}\label{classWBC__priority_a8c838694a7fa3038bb56923679050bea}} 
\index{WBC\_priority@{WBC\_priority}!WBC\_priority@{WBC\_priority}}
\index{WBC\_priority@{WBC\_priority}!WBC\_priority@{WBC\_priority}}
\doxysubsubsection{\texorpdfstring{WBC\_priority()}{WBC\_priority()}}
{\footnotesize\ttfamily W\+B\+C\+\_\+priority\+::\+W\+B\+C\+\_\+priority (\begin{DoxyParamCaption}\item[{int}]{model\+\_\+nv\+\_\+\+In,  }\item[{int}]{Q\+P\+\_\+nv\+In,  }\item[{int}]{Q\+P\+\_\+nc\+In,  }\item[{double}]{miu\+\_\+\+In,  }\item[{double}]{dt }\end{DoxyParamCaption})}



Construct a new wbc priority object. 


\begin{DoxyParams}{Parameters}
{\em model\+\_\+nv\+\_\+\+In} & model degree of freedom \\
\hline
{\em Q\+P\+\_\+nv\+In} & dimension of optimized variable of W\+BC QP problem \\
\hline
{\em Q\+P\+\_\+nc\+In} & dimension of constraints of W\+BC QP problem \\
\hline
{\em miu\+\_\+\+In} & friction coefficient of model \\
\hline
{\em dt} & control horizon \\
\hline
\end{DoxyParams}


Definition at line 12 of file wbc\+\_\+priority.\+cpp.



References Priority\+Tasks\+::add\+Task(), Priority\+Tasks\+::build\+Priority(), ddq\+\_\+final\+\_\+kin, delta\+\_\+q\+\_\+final\+\_\+kin, dq\+\_\+final\+\_\+kin, eigen\+\_\+ddq\+\_\+\+Opt, eigen\+\_\+fr\+\_\+\+Opt, eigen\+\_\+tau\+\_\+\+Opt, eigen\+\_\+x\+Opt, f\+\_\+z\+\_\+low, f\+\_\+z\+\_\+upp, kin\+\_\+tasks, miu, model\+\_\+nv, tau\+\_\+low\+\_\+L, and tau\+\_\+upp\+\_\+L.



\doxysubsection{Member Function Documentation}
\mbox{\Hypertarget{classWBC__priority_a7cb65e689671b04b1b4bca685602abaa}\label{classWBC__priority_a7cb65e689671b04b1b4bca685602abaa}} 
\index{WBC\_priority@{WBC\_priority}!computeDdq@{computeDdq}}
\index{computeDdq@{computeDdq}!WBC\_priority@{WBC\_priority}}
\doxysubsubsection{\texorpdfstring{computeDdq()}{computeDdq()}}
{\footnotesize\ttfamily void W\+B\+C\+\_\+priority\+::compute\+Ddq (\begin{DoxyParamCaption}\item[{\mbox{\hyperlink{classPin__KinDyn}{Pin\+\_\+\+Kin\+Dyn}} \&}]{kin\+Solver }\end{DoxyParamCaption})}



compute the desired dq, basically the Kin\+W\+BC 



Definition at line 287 of file wbc\+\_\+priority.\+cpp.



References Priority\+Tasks\+::compute\+All(), Pin\+\_\+\+Kin\+Dyn\+::compute\+In\+K\+\_\+\+Hand(), ddq\+\_\+final\+\_\+kin, delta\+\_\+q\+\_\+final\+\_\+kin, d\+J\+\_\+hd\+\_\+l, d\+J\+\_\+hd\+\_\+r, d\+Jc, d\+Jsw, dq, dq\+\_\+final\+\_\+kin, dyn\+\_\+M, dyn\+\_\+\+M\+\_\+inv, fe\+\_\+pos\+\_\+sw\+\_\+W, fe\+\_\+rot\+\_\+sw\+\_\+W, Priority\+Tasks\+::get\+Id(), hd\+\_\+l\+\_\+pos\+\_\+cur\+\_\+W, hd\+\_\+l\+\_\+rot\+\_\+cur\+\_\+W, hd\+\_\+r\+\_\+pos\+\_\+cur\+\_\+W, hd\+\_\+r\+\_\+rot\+\_\+cur\+\_\+W, J\+\_\+hd\+\_\+l, J\+\_\+hd\+\_\+r, Jc, Jsw, kin\+\_\+tasks, model\+\_\+nv, Priority\+Tasks\+::out\+\_\+ddq, Priority\+Tasks\+::out\+\_\+delta\+\_\+q, Priority\+Tasks\+::out\+\_\+dq, q, and Priority\+Tasks\+::task\+Lib.

\mbox{\Hypertarget{classWBC__priority_adf9a6a3385417332666e2157b6903fe9}\label{classWBC__priority_adf9a6a3385417332666e2157b6903fe9}} 
\index{WBC\_priority@{WBC\_priority}!computeTau@{computeTau}}
\index{computeTau@{computeTau}!WBC\_priority@{WBC\_priority}}
\doxysubsubsection{\texorpdfstring{computeTau()}{computeTau()}}
{\footnotesize\ttfamily void W\+B\+C\+\_\+priority\+::compute\+Tau (\begin{DoxyParamCaption}{ }\end{DoxyParamCaption})}



compute the desired joint torques, basically the Dyn\+W\+BC 



Definition at line 151 of file wbc\+\_\+priority.\+cpp.



References ddq\+\_\+final\+\_\+kin, dyn\+\_\+M, dyn\+\_\+\+Non, eigen\+\_\+ddq\+\_\+\+Opt, eigen\+\_\+fr\+\_\+\+Opt, eigen\+\_\+x\+Opt, f\+\_\+z\+\_\+low, f\+\_\+z\+\_\+upp, fe\+\_\+\+L\+\_\+rot\+\_\+\+L\+\_\+off, Fr\+\_\+ff, Jfe, leg\+State\+Cur, Data\+Bus\+::\+L\+St, miu, model\+\_\+nv, Q1, Q2, tau\+\_\+low\+\_\+L, tau\+\_\+upp\+\_\+L, and tau\+Joint\+Res.

\mbox{\Hypertarget{classWBC__priority_a7ed27e44e3d0044570620d3679f26389}\label{classWBC__priority_a7ed27e44e3d0044570620d3679f26389}} 
\index{WBC\_priority@{WBC\_priority}!dataBusRead@{dataBusRead}}
\index{dataBusRead@{dataBusRead}!WBC\_priority@{WBC\_priority}}
\doxysubsubsection{\texorpdfstring{dataBusRead()}{dataBusRead()}}
{\footnotesize\ttfamily void W\+B\+C\+\_\+priority\+::data\+Bus\+Read (\begin{DoxyParamCaption}\item[{const \mbox{\hyperlink{structDataBus}{Data\+Bus}} \&}]{robot\+State }\end{DoxyParamCaption})}



read data from databus 


\begin{DoxyParams}{Parameters}
{\em robot\+State} & \\
\hline
\end{DoxyParams}


Definition at line 73 of file wbc\+\_\+priority.\+cpp.



References Data\+Bus\+::base\+\_\+pos, Data\+Bus\+::base\+\_\+pos\+\_\+des, Data\+Bus\+::base\+\_\+rot, Data\+Bus\+::base\+\_\+rpy\+\_\+des, Data\+Bus\+::des\+\_\+ddq, Data\+Bus\+::des\+\_\+delta\+\_\+q, Data\+Bus\+::des\+\_\+dq, Data\+Bus\+::des\+\_\+q, Data\+Bus\+::d\+J\+\_\+base, d\+J\+\_\+hd\+\_\+l, d\+J\+\_\+hd\+\_\+r, Data\+Bus\+::d\+J\+\_\+l, Data\+Bus\+::d\+J\+\_\+r, d\+Jc, d\+Jsw, Data\+Bus\+::dq, dq, dyn\+\_\+\+Ag, Data\+Bus\+::dyn\+\_\+\+Ag, dyn\+\_\+d\+Ag, Data\+Bus\+::dyn\+\_\+d\+Ag, dyn\+\_\+M, Data\+Bus\+::dyn\+\_\+M, dyn\+\_\+\+M\+\_\+inv, Data\+Bus\+::dyn\+\_\+\+M\+\_\+inv, dyn\+\_\+\+Non, Data\+Bus\+::dyn\+\_\+\+Non, Data\+Bus\+::fe\+\_\+l\+\_\+pos\+\_\+W, Data\+Bus\+::fe\+\_\+\+L\+\_\+rot\+\_\+\+L\+\_\+off, fe\+\_\+\+L\+\_\+rot\+\_\+\+L\+\_\+off, Data\+Bus\+::fe\+\_\+l\+\_\+rot\+\_\+W, fe\+\_\+pos\+\_\+sw\+\_\+W, Data\+Bus\+::fe\+\_\+r\+\_\+pos\+\_\+W, Data\+Bus\+::fe\+\_\+\+R\+\_\+rot\+\_\+\+L\+\_\+off, fe\+\_\+\+R\+\_\+rot\+\_\+\+L\+\_\+off, Data\+Bus\+::fe\+\_\+r\+\_\+rot\+\_\+W, fe\+\_\+rot\+\_\+sw\+\_\+W, Fr\+\_\+ff, Data\+Bus\+::\+Fr\+\_\+ff, hd\+\_\+l\+\_\+pos\+\_\+cur\+\_\+W, Data\+Bus\+::hd\+\_\+l\+\_\+pos\+\_\+W, hd\+\_\+l\+\_\+rot\+\_\+cur\+\_\+W, Data\+Bus\+::hd\+\_\+l\+\_\+rot\+\_\+W, hd\+\_\+r\+\_\+pos\+\_\+cur\+\_\+W, Data\+Bus\+::hd\+\_\+r\+\_\+pos\+\_\+W, hd\+\_\+r\+\_\+rot\+\_\+cur\+\_\+W, Data\+Bus\+::hd\+\_\+r\+\_\+rot\+\_\+W, Data\+Bus\+::\+J\+\_\+base, J\+\_\+hd\+\_\+l, Data\+Bus\+::\+J\+\_\+hd\+\_\+l, J\+\_\+hd\+\_\+r, Data\+Bus\+::\+J\+\_\+hd\+\_\+r, Data\+Bus\+::\+J\+\_\+l, Data\+Bus\+::\+J\+\_\+r, Jc, Jfe, Jsw, Data\+Bus\+::leg\+State, leg\+State\+Cur, Data\+Bus\+::\+L\+St, model\+\_\+nv, Data\+Bus\+::q, q, Data\+Bus\+::rpy, Data\+Bus\+::stance\+\_\+fe\+\_\+pos\+\_\+cur\+\_\+W, Data\+Bus\+::stance\+\_\+fe\+\_\+rot\+\_\+cur\+\_\+W, Data\+Bus\+::stance\+Des\+Pos\+\_\+W, Data\+Bus\+::swing\+\_\+fe\+\_\+pos\+\_\+des\+\_\+W, and Data\+Bus\+::swing\+\_\+fe\+\_\+rpy\+\_\+des\+\_\+W.

\mbox{\Hypertarget{classWBC__priority_a1504c45533bea5c9b6a4a87648db0c0c}\label{classWBC__priority_a1504c45533bea5c9b6a4a87648db0c0c}} 
\index{WBC\_priority@{WBC\_priority}!dataBusWrite@{dataBusWrite}}
\index{dataBusWrite@{dataBusWrite}!WBC\_priority@{WBC\_priority}}
\doxysubsubsection{\texorpdfstring{dataBusWrite()}{dataBusWrite()}}
{\footnotesize\ttfamily void W\+B\+C\+\_\+priority\+::data\+Bus\+Write (\begin{DoxyParamCaption}\item[{\mbox{\hyperlink{structDataBus}{Data\+Bus}} \&}]{robot\+State }\end{DoxyParamCaption})}



write data to databus 


\begin{DoxyParams}{Parameters}
{\em robot\+State} & \\
\hline
\end{DoxyParams}


Definition at line 138 of file wbc\+\_\+priority.\+cpp.



References ddq\+\_\+final\+\_\+kin, delta\+\_\+q\+\_\+final\+\_\+kin, dq\+\_\+final\+\_\+kin, eigen\+\_\+fr\+\_\+\+Opt, Data\+Bus\+::qp\+\_\+cpu\+Time, Data\+Bus\+::qp\+\_\+n\+W\+SR, Data\+Bus\+::qp\+\_\+status, tau\+Joint\+Res, Data\+Bus\+::wbc\+\_\+ddq\+\_\+final, Data\+Bus\+::wbc\+\_\+delta\+\_\+q\+\_\+final, Data\+Bus\+::wbc\+\_\+dq\+\_\+final, Data\+Bus\+::wbc\+\_\+\+Fr\+Res, and Data\+Bus\+::wbc\+\_\+tau\+Joint\+Res.

\mbox{\Hypertarget{classWBC__priority_ad7a8bfff2aa05dcedeb6e3e656d3c6fd}\label{classWBC__priority_ad7a8bfff2aa05dcedeb6e3e656d3c6fd}} 
\index{WBC\_priority@{WBC\_priority}!setQini@{setQini}}
\index{setQini@{setQini}!WBC\_priority@{WBC\_priority}}
\doxysubsubsection{\texorpdfstring{setQini()}{setQini()}}
{\footnotesize\ttfamily void W\+B\+C\+\_\+priority\+::set\+Qini (\begin{DoxyParamCaption}\item[{const Eigen\+::\+Vector\+Xd \&}]{q\+Ini }\end{DoxyParamCaption})}



Set the initial generalized position of robot. 


\begin{DoxyParams}{Parameters}
{\em q\+Ini} & initial generalized position of robot \\
\hline
\end{DoxyParams}


Definition at line 496 of file wbc\+\_\+priority.\+cpp.



The documentation for this class was generated from the following files\+:\begin{DoxyCompactItemize}
\item 
wbc\+\_\+priority.\+h\item 
wbc\+\_\+priority.\+cpp\end{DoxyCompactItemize}
